/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;
import Behaviors.Schemas.Perception.PSMapVP;
import Util.VectorFrame;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Controllers.*;
import Robot.*;
import Robot.Sensors.*;
import javaclient2.structures.PlayerConstants;
import Util.*;
import javaclient2.*;
import javax.swing.*;

/**
 *
 * @author alejo
 */
public class pruebas {
    
    public static void main(String args[])
    {

        boolean isSimulated = false;
        boolean isUsingVp = true;
        boolean showArbFrame = true;
        boolean showMapObstFrame = false;


        Robot real;
        Sensor ir;
        //Creacion de la instancia robot.
        if(isSimulated)
            real = new Robot("localhost",6665,"real",0);
        else
            real = new Robot("192.168.0.1",6665,"real",0);

        RealPosition2d posReal;
        
        
        try{
        
            posReal = new RealPosition2d(real,"realPos",0);
    
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }

        




        try{
            if(isSimulated)
                ir = new Sonar(real,"realIR",0);
            else
                ir = new IR(real,"realIR",0);

    
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }

           
        //VAnish point sensor
        VPSensor vp = new VPSensor(640,480,"localhost",25557);
        if(isUsingVp)
            vp.start();

        real.runThreaded(-1, -1);

        if(isSimulated)
            while(!((Sonar)ir).updatePoses());
        else
            while(!((IR)ir).updatePoses());

        
        posReal.setMotorPower(true);

        Pose2d initialPose;
        if(isSimulated)
            initialPose = new Pose2d((float)15,(float)25,(float)-Math.PI/2);
        else
            initialPose = new Pose2d(0.0f,0.0f,0);

        posReal.setInitialPosition(initialPose);

        if(isSimulated)
        {
            posReal.setMaxSpeed(0.4f);
            posReal.setRotControl(0.3f,1.0f);
        }
        else
        {
            posReal.setMaxSpeed(1.0f);
            posReal.setRotControl(0.2f,0.2f);
        }



        Vector2d point;

        if(isSimulated)
            point=new Vector2d(15.0f,19.5f,Vector2d.MODE_XY);
        else
            point=new Vector2d(2.23f,0.0f,Vector2d.MODE_XY);

        MSGoToPoint goto2 = new MSGoToPoint("goto",posReal,point,MSGoToPoint.MODE_EXPONENTIAL);


        PSDetectObstacles obstReal;

        if(isSimulated)
            obstReal = new PSDetectObstaclesSonar("obstReal",posReal, (Sonar)ir, true  );
        else
            obstReal = new PSDetectObstaclesIR("obstReal",posReal, (IR)ir);

        PSMapObstacles obstMapped = new PSMapObstacles("6mapObstacles",obstReal, posReal,showMapObstFrame);
        
        PSMapObstacles obstMapped2 = new PSMapObstacles("mapObstacles",obstReal, posReal,0.3f,-1,showMapObstFrame);
        
        
        MSAvoidObstacles avoidReal = new MSAvoidObstacles("avoidReal",obstMapped2, posReal,MSAvoidObstacles.MODE_EXPONENTIAL);


        avoidReal.setExponentialDistance(0.3f);

        MSGoForward forward = new MSGoForward("forward",posReal);

        MSAvoidObstaclesVFH avoid = new MSAvoidObstaclesVFH("avoidVFH",obstMapped,goto2,posReal,MSAvoidObstacles.MODE_LINEAR);

        MSNoise noise = new MSNoise("noise");
        
        PSMapVP vanishPoint;
        
        vanishPoint = new PSMapVP("vanish",vp,posReal);
        vanishPoint.setGain(0.1f);

        
        goto2.setGain(1.0f);
        avoidReal.setGain(0.4f);
        avoid.setGain(0.7f);
        noise.setGain(0.4f);


        forward.setGain(0.6f);

        Schema[] array = {goto2};
        //Schema[] array = {forward,vanishPoint};//,avoidReal};


        

        VectorAdditionArbitrator arbitrator1=new VectorAdditionArbitrator(array,posReal,showArbFrame);
        
        //arbitrator1.setDebugMode(true);
        arbitrator1.enabled = true;
        


        
        
        try
        {
        Thread.sleep(2000);
        }
        catch(Exception e)
        {
            e.printStackTrace();
        }

        

        while(true)
        {
         
            arbitrator1.step();

            int fidelity = 0;
            int oldFid = 0;
            int counts = 0;
            if(isUsingVp)
            {
                Vector2d newGoalPoint = vanishPoint.Value();
                fidelity = vanishPoint.getFidelity();

                if(newGoalPoint != null)
                {
                    System.out.println("Vector de puntoDesv:");
                    newGoalPoint.print(Vector2d.MODE_XY);
                    System.out.println("Posicion Robot:");
                    posReal.getPositionVector().print(Vector2d.MODE_XY);

                    System.out.println("fidelity: "+fidelity);

                    if(fidelity > oldFid)
                    {
                        goto2.changeGoalPoint(newGoalPoint);
                        oldFid = fidelity;
                        counts = 0;
                    }
                    else if (counts>100)
                    {   
                        goto2.changeGoalPoint(newGoalPoint);
                        oldFid = fidelity;
                        counts = 0;
                    }
                    else
                        counts++;

                }
            }
            try
            {
                Thread.sleep(50);
            }
            catch(Exception e)
            {}
        }
        
        /*
        
      
        while(true)
        {

            float[] result = null;
            while(result == null)
                result = ir.read(Sensor.READ_MODE_WAIT);


            
            int resultadoIR = 0;
            for(int i=0; i<result.length;i++)
            {
                if(result[i] == 0)
                {
                    switch(i)
                    {
                        case 0:
                            resultadoIR += 1;
                            break;
                        case 1:
                            resultadoIR += 2;
                            break;
                        case 2:
                            resultadoIR += 4;
                            break;          
                        case 3:
                            resultadoIR += 8;
                            break;          
                        case 4:
                            resultadoIR += 16;
                            break;           
                        case 5:
                            resultadoIR += 32;
                            break;                            
                    }
                }
                
                
            }
            
            System.out.println(resultadoIR);
            
            
            switch(resultadoIR)
            {
                case 1:
                case 2:
                case 3:
                case 6:
                case 7:
                case 15:
                case 31:
                    posReal.setSpeed(0, -0.8f);
                    break;
                
                case 4:
                case 8:
                case 12:
                case 30:
                
                    posReal.setSpeed(-0.3f,0.8f);
                    break;
                case 16:
                case 32:
                case 48:
                case 24:
                case 56:
                case 28:
                case 60:
                case 62:
                    posReal.setSpeed(0, 0.8f);
                    break;
                default:
                    posReal.setSpeed(0.3f, 0);
            }
        }*/
     
    }

}
